// Experiment with fixed point math

#define FP 12
#define BAUD 38400  

// Define pin assignments
#define LEDPIN 13
#define XACCELPIN 0 // I miswired my board, change order based on your setup
#define YACCELPIN 1
#define ZACCELPIN 2
#define XRATEPIN 4
#define YRATEPIN 3
#define ZRATEPIN 5

// Accelerometer setup
// these A/D values depend on how well the sensors are mounted
// change these values to your unique configuration
#define XMIN 405
#define XMAX 607
#define YMIN 409
#define YMAX 618
#define ZMIN 403
#define ZMAX 611

// Alternate accelerometer setup
// gx = mx(X) + bx, where X is analogRead() value for x accelerometer
float gx, gy, gz;
float mx = 2 / float(XMAX - XMIN);
float bx = 1 - float(mx * XMAX);
float my = 2 / float(YMAX - YMIN);
float by = 1 - float(my * YMAX);
float mz = 2 / float(ZMAX - ZMIN);
float bz = 1 - float(mz * ZMAX);
// rho = roll angle of y-axis relative to the ground
// phi = pitch angle of x-axis relative to the ground
// theta = angle of z-axis relative to gravity
float roll, pitch, zAxis;
float PiOver2 = PI/2;

// Gyro setup
// if AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
float GyroMid = 465;
// gyro range is from +/-500 deg/sec
float GyroScaleFactor = 1001 / (float)ZMAX;
int xGyroZero, yGyroZero, zGyroZero;
float xRate, yRate, zRate;

unsigned long previousTime = 0;
unsigned long deltaTime = 0;
int x, y, z;

void setup() {
  digitalWrite(LEDPIN, HIGH);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  Serial.begin(BAUD);
}

void loop() {

  x = analogRead(XACCELPIN);
  y = analogRead(YACCELPIN);
  z = analogRead(ZACCELPIN);
  
  previousTime = millis();
  gx = (mx*x) + bx;
  gy = (my*y) + by;
  gz = (mz*z) +bz;
  roll = atan2(gy, gz);
  pitch = atan2(-gx, gz);
  zAxis = atan2(sqrt(gx*gx+gy*gy),gz);
  deltaTime = millis() - previousTime;

  // measuredRate = (RATE-GYROMID+ZERO)*(1000/MAX)
  xRate = ((float)analogRead(XRATEPIN) - GyroMid + xGyroZero);// * GyroScaleFactor;
  yRate = ((float)analogRead(YRATEPIN) - GyroMid + yGyroZero);// * GyroScaleFactor;
  zRate = ((float)analogRead(YRATEPIN) - GyroMid + zGyroZero);// * GyroScaleFactor;
  
  xmitData();
}

float findZero(int channel) {
  float zero = 0;
  for (int i=0; i< 100; i++) zero += analogRead(channel);
  return (zero / 100);
}  

void xmitData() {
  Serial.print(int(deltaTime));
  Serial.print(',');
  Serial.print(int(x));
  Serial.print(',');
  Serial.print(int(y));
  Serial.print(',');
  Serial.print(int(z));
  Serial.print(',');
  Serial.print(int(gx*1000));
  Serial.print(',');
  Serial.print(int(gy*1000));
  Serial.print(',');
  Serial.print(int(gz*1000));
  Serial.print(',');
  Serial.print(int(roll*1000));
  Serial.print(',');
  Serial.print(int(pitch*1000));
  Serial.print(',');
  Serial.print(int(zAxis*1000));
  Serial.print(',');
  Serial.print(int(xRate*1000));
  Serial.print(',');
  Serial.print(int(yRate*1000));
  Serial.print(',');
  Serial.println(int(zRate*1000));
 // delay(500);
}

int smooth(int data, float filterVal, float smoothedVal){


  if (filterVal > 1){      // check to make sure param's are within range
    filterVal = .99;
  }
  else if (filterVal <= 0){
    filterVal = 0;
  }

  smoothedVal = (data * (1 - filterVal)) + (smoothedVal  *  filterVal);

  return (int)smoothedVal;
}
/*
int R_ANGLE = 1229; //(int) (0.3 * 4096);
int Q_ANGLE = 4; //(int) (0.001 * 4096);
int Q_GYRO = 12; //(int) (0.003 * 4096);

int angle = 0;
int q_bias = 0;
int rate = 0;
int P0 = 1 << FP;
int P1 = 0 << FP;
int P2 = 0 << FP;
int P3 = 1 << FP;


// q_m must be prescaled to FP
void predict(int q_m, int deltaTime) {
  int dt = (deltaTime << FP) / 512; // (deltaTime is ms since last time) 
  rate = q_m - q_bias;
  angle += ((rate * dt) >> FP);
  P0 += ((Q_ANGLE - P1 - P2) * dt) >> FP;
  P1 += (-P3 * dt) >> FP;
  P2 += (-P3 * dt) >> FP;
  P3 += (Q_GYRO * dt) >> FP;
}

// angle_m must not be prescaled to FP
int update(int angle_m) {
  int E = R_ANGLE + P0;
  int K0 = (P0 << FP) / E;
  int K1 = (P2 << FP) / E;
  int T0 = P0;
  int T1 = P1;
  P0 -= ((K0 * T0) >> FP);
  P1 -= ((K0 * T1) >> FP);
  P2 -= ((K1 * T0) >> FP);
  P3 -= ((K1 * T1) >> FP);

  int angle_err = (angle_m << FP) - angle;
  angle  += ((K0 * angle_err) >> FP);
  q_bias += ((K1 * angle_err) >> FP);

  return angle >> FP;
}

// Inspired by:  http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
int arctan2(int y, int x) {
  int angle = 0;
  //    int coeff_1 = 128; // PI/4 = 512/4 (2*PI rads = 1024 quids = 360 degs)
  //    int coeff_2 = 384; // coeff_1 * 3;
  int abs_y = abs(y);
  if(x>=0) {
    int r = 0;
    if((x+abs_y)!=0) { // Avoiding div by zero
      r = ((x - abs_y) << FP) / (x + abs_y);
    }
    angle = 128 - (r >> (FP-7));
  } 
  else {
    int r = 0;
    if((abs_y-x)!=0) { // Avoiding div by zero
      r = ((x + abs_y) << FP) / (abs_y - x);
    }
    angle = 384 - (r >> (FP-7));
  }
  if(y<0) angle*=-1;
  return angle;
}

void setup() {
  //nothing
}

void loop (){
  //nothing
}
*/
